#!/usr/bin/env python3
import rospy
import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import PointStamped

class ObjectWorldPublisher:
    def __init__(self):
        rospy.init_node('object_world_publisher')
        self.tf_buffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.tf_buffer)
        self.pub = rospy.Publisher('/yolo/object_world', PointStamped, queue_size=1)

        rospy.Subscriber('/yolo/object_3d', PointStamped, self.callback)
        rospy.loginfo("Listening to /yolo/object_3d and transforming to world frame...")

    def callback(self, msg):
        try:
            # 从相机坐标系转换到 base_link 或 world 坐标系
            trans = self.tf_buffer.lookup_transform('base_link',  # 目标坐标系（世界）
                                                    msg.header.frame_id,  # 源坐标系（相机）
                                                    rospy.Time(0),  # 最新时间
                                                    rospy.Duration(0.5))
            point_world = tf2_geometry_msgs.do_transform_point(msg, trans)
            self.pub.publish(point_world)

            rospy.loginfo_throttle(1.0, f"Object in world: {point_world.point}")
        except Exception as e:
            rospy.logwarn_throttle(2.0, f"TF transform failed: {e}")

if __name__ == "__main__":
    ObjectWorldPublisher()
    rospy.spin()
